package com.bodystm.algorithm;
/***
 * 中值滤波算法类
 * @author a
 *
 */
public class MedFilter {
	private final int LEN1 = 55;//533*0.3/2
    private final int LEN2 = 109;//533*0.6/2
    double[] x3 = new double[2 * LEN1 + 1];//第一次中值滤波排序数组
    double[] x4 = new double[2 * LEN2 + 1];//第二次中值滤波排序数组
    double[] px3 = new double[2 * LEN1 + 1];//第一次中值滤波前原输入数组（输入值的定长数组）
    double[] px4 = new double[2 * LEN2 + 1];//第二次中值滤波前原输入数组（第一次中值滤波输出的定长数组）
    double[] px5 = new double[2 * LEN2 + 1];
    int icount3 = 0;//第一次中值滤波计数器
    int icount4 = 0;//第二次中值滤波计数器

    public MedFilter()
    {

    }
    private double[] qSort(double[] x, int low, int high)
    {
        int i, j;double s;
        if (low < high)
        {
            i = low;
            j = high;
            s = x[i];
            while (i < j)
            {
                while (i < j && x[j] > s) j--; /* 从右向左找第一个小于x的数 */
                if (i < j) x[i++] = x[j];
                while (i < j && x[i] < s) i++; /* 从左向右找第一个大于x的数 */
                if (i < j) x[j--] = x[i];
            }
            x[i] = s;
            x=qSort(x, low, i - 1); /* 递归调用 */
            x=qSort(x, i + 1, high);
        }
        return x;
    }

    private int iBinSearch(double[] sSource, int l, int h, double key)
    {

        int low = l, high = h, mid;
        while (low <= high)
        {
            mid = (low + high) / 2;//获取中间的位置

            if (sSource[mid] > key)
                high = mid - 1; //如果比key大，则往低的位置查找
            else if (sSource[mid] < key)
                low = mid + 1;  //如果比key小，则往高的位置查找
            else
                return mid; //找到则返回相应的位置
        }
        return high;
    }
    /// <summary>
    /// 中值滤波函数
    /// </summary>
    /// <param name="x"></param>
    /// <returns></returns>
    public double MedFilterFunc(double x)
    {
        //第一次中值滤波
        int length = 0;
        int temp = 0; double outvar = 0;
        int ppp = 0;
        int index = 0;
        double medianvalue1, medianvalue2;
        int ib, invar;

        if (icount3 >= 2 * (2 * LEN2 + 1) * (2 * LEN1 + 1))
            icount3 -= (2 * LEN2 + 1) * (2 * LEN1 + 1);

        if (icount3 < LEN1 + 1)
        {
            x3[icount3] = x;
            px3[icount3] = x;
            px5[icount3] = x;
            icount3++;
            if (icount3 == LEN1 + 1)
            {
            	x3=qSort(x3, 0, LEN1);
                length = LEN1 + 1;
                if (length % 2 == 1)
                {
                    index = (length + 1) / 2;
                    outvar = x3[index - 1];
                }
                else
                {
                    index = length / 2;
                    medianvalue1 = x3[index - 1];
                    medianvalue2 = x3[index];
                    outvar = (medianvalue1 + medianvalue2) / 2;
                }
            }
        }
        else if (icount3 < 2 * LEN1 + 1)
        {
            length = icount3 + 1;
            invar = iBinSearch(x3, 0, icount3 - 1, x);
            ib = icount3;
            ++invar;
            while (invar < ib)
            {
                x3[ib] = x3[ib - 1];
                ib--;
            }
            x3[invar] = x;

            if (length % 2 == 1)
            {
                index = (length + 1) / 2;
                outvar = x3[index - 1];
            }
            else
            {
                index = length / 2;
                medianvalue1 = x3[index - 1];
                medianvalue2 = x3[index];
                outvar = (medianvalue1 + medianvalue2) / 2;
            }
            px3[icount3] = x;
            px5[icount3] = x;
            icount3++;
        }
        else
        {
            length = 2 * LEN1 + 1;
            temp = icount3;
            while (temp >= length)
                temp -= length;

            ppp = icount3;
            while (ppp >= (2 * LEN2 + 1))
                ppp -= (2 * LEN2 + 1);

            ib = iBinSearch(x3, 0, length - 1, px3[temp]);
            invar = iBinSearch(x3, 0, length - 1, x);
            if (invar < ib)
            {
                ++invar;
                while (invar < ib)
                {
                    x3[ib] = x3[ib - 1];
                    ib--;
                }
            }
            else
            {
                while (invar > ib)
                {
                    x3[ib] = x3[ib + 1];
                    ib++;
                }
            }
            x3[invar] = x;
            px3[temp] = x;
            px5[ppp] = x;
            icount3++;
            outvar = x3[LEN1];
        }
        //第二次中值滤波
        int length1 = 0;
        int temp1 = 0; double outvar1 = 0;

        if (icount4 >= 2 * (2 * LEN2 + 1) + LEN1)
            icount4 -= (2 * LEN2 + 1);

        if (icount4 < LEN1)
        {
            icount4++;
            return 0;
        }
        else if (icount4 < LEN1 + LEN2 + 1)
        {
            x4[icount4 - LEN1] = outvar;
            px4[icount4 - LEN1] = outvar;
            icount4++;
            if (icount4 == LEN1 + LEN2 + 1)
            {
            	x4=qSort(x4, 0, LEN2);
                length1 = LEN2 + 1;
                if (length1 % 2 == 1)
                {
                    index = (length1 + 1) / 2;
                    outvar1 = x4[index - 1];
                }
                else
                {
                    index = length1 / 2;
                    medianvalue1 = x4[index - 1];
                    medianvalue2 = x4[index];
                    outvar1 = (medianvalue1 + medianvalue2) / 2;
                }
                return (px5[0] - outvar1);
            }
            else
                return 0;
        }
        else if (icount4 < (2 * LEN2 + LEN1 + 1))
        {
            length1 = icount4 - LEN1 + 1;
            invar = iBinSearch(x4, 0, length1 - 2, outvar);
            ib = icount4 - LEN1;
            ++invar;
            while (invar < ib)
            {
                x4[ib] = x4[ib - 1];
                ib--;
            }
            x4[invar] = outvar;

            if (length1 % 2 == 1)
            {
                index = (length1 + 1) / 2;
                outvar1 = x4[index - 1];
            }
            else
            {
                index = length1 / 2;
                medianvalue1 = x4[index - 1];
                medianvalue2 = x4[index];
                outvar1 = (medianvalue1 + medianvalue2) / 2;
            }
            px4[icount4 - LEN1] = outvar;
            icount4++;
            return (px5[icount4 - LEN2 - 1 - LEN1] - outvar1);
        }
        else
        {
            length1 = 2 * LEN2 + 1;
            temp1 = icount4 - LEN1;
            while (temp1 >= length1)
                temp1 -= length1;
            ib = iBinSearch(x4, 0, length1 - 1, px4[temp1]);
            invar = iBinSearch(x4, 0, length1 - 1, outvar);
            if (invar < ib)
            {
                ++invar;
                while (invar < ib)
                {
                    x4[ib] = x4[ib - 1];
                    ib--;
                }
            }
            else
            {
                while (invar > ib)
                {
                    x4[ib] = x4[ib + 1];
                    ib++;
                }
            }
            x4[invar] = outvar;
            px4[temp1] = outvar;
            icount4++;
            outvar1 = x4[LEN2];
            return (px5[(temp1 < LEN2) ? (temp1 + LEN2 + 1) : (temp1 - LEN2)] - outvar1);
        }
    }
}
